/**
  ******************************************************************************
  * @file    main.cc
  * @author  xinkai yu
  * @version V1.0.2
  * @date    2022/02/22
  * @brief   arm logical decision node for MobiRo gen3 @ TIB330
  ******************************************************************************
  * @attention
  *
  ******************************************************************************
  */

#include <iostream>
#include <thread>
#include <mutex>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include "ros/ros.h"
#include "tf/tf.h"
#include "tf_conversions/tf_eigen.h"
#include "geometry_msgs/TransformStamped.h"
#include "std_msgs/UInt8.h"
#include "std_srvs/SetBool.h"
#include "std_srvs/Empty.h"

#include "jsk_recognition_msgs/BoundingBox.h"

using namespace Eigen;

class ArmDecision {
 public:
  explicit ArmDecision() {
   //这是得到目标物体的xyz
    target_pose_sub_ = nh_.subscribe(
        "target_pose", 1, &ArmDecision::TargetPoseCallback, this);

    detect_time_sub_ = nh_.subscribe(
        "grasp_target", 1, &ArmDecision::DetectTimeCallback, this);

    //这是得到爪子的xyz和yaw，这是为了判断跟上面这个向量的距离大小
    hand_pose_sub_ = nh_.subscribe(
        "hand_pose", 1, &ArmDecision::HandPoseCallback, this);

    //这就是call了爪子闭合的服务
    grip_action_ = nh_.serviceClient<std_srvs::SetBool>(
        "arm_base/set_gripper_status");

    kine_unlock_ = nh_.serviceClient<std_srvs::Empty>("arm_kinematic/unlock");

    //这个话题是发布搜寻的方向，分为1和2
    arm_search_pub_ = nh_.advertise<std_msgs::UInt8>("target_search", 1);

    //这个线程时刻判断距离是不是足够近了，如果足够近，就执行抓取，这种方式，是否比判断到达目标位置，更好
    //1、判断是否够近，然后call爪子闭合服务 2、当现在超过上一次检测时间5秒后，进行搜寻
    reach_moniter_thread_ = std::thread(&ArmDecision::TargetReachMoniter, this);
    target_position_ = Vector3d(0.0, 0.0, 0.0);
    hand_yaw_ = 0.0;
    last_hand_yaw_ = 0.0;
    grip_enable_flag_ = true;
    nav_ongoing_flag_ = false;
    last_detect_time_ = ros::Time::now();
  }

  ~ArmDecision() {
    if (reach_moniter_thread_.joinable()) reach_moniter_thread_.join();
  }

  void TargetReachMoniter() {  // change to single thread
    ros::Rate loop_rate(100);
    while (ros::ok()) {
      ros::spinOnce();
      if ((target_position_ - hand_position_).norm() < 0.1 &&
          target_position_.norm() != 0) {
        // if (grip_enable_flag_) {//本来就是true，很有可能当时这个是false，导致没闭合
          // std_srvs::SetBool grip_cmd;
          // grip_cmd.request.data = true;
          // grip_action_.call(grip_cmd);//也有可能服务没call成功
          // std::cout << grip_cmd.response.message << std::endl;
          // grip_enable_flag_ = false;  // grasp ongoing avoid interruption，避免在抓取的时候打断
          // target_position_ = Vector3d(0.0, 0.0, 0.0);//重置target的坐标，这样就不会重新抓取了，进不了这个if
          // ROS_INFO("Target reach, hand grip action!");
        // }
        loop_rate.sleep();
        continue;
      }
      else if (!grip_enable_flag_) {  // going default after grasp
        grip_enable_flag_ = true;
        std_srvs::Empty unlock_cmd;
        kine_unlock_.call(unlock_cmd);//这个服务证明抓取已经抓完了，反馈回去，让那边可以重置是否发布目标关节角度
        loop_rate.sleep();
        continue;
      }

      // run into scout mode
      if ((ros::Time::now() - last_detect_time_).toSec() > 5) {
        std::cout << "Hand yaw: " << hand_yaw_ << " " 
                  << "last yaw: " << last_hand_yaw_ 
                  << std::endl;
        std_msgs::UInt8 search_cmd;
        if (hand_yaw_ <= last_hand_yaw_) {
          if (hand_yaw_ - 15.0 > -50.0) search_cmd.data = 1;  // clockwise search
          else search_cmd.data = 2;  // counterclockwise search
        }
        else {
          if (hand_yaw_ + 15.0 < 50.0) search_cmd.data = 2;  // counterclockwise search
          else search_cmd.data = 1;  // clockwise search
        }
        if(!nav_ongoing_flag_){
        arm_search_pub_.publish(search_cmd);
        }
        last_detect_time_ = ros::Time::now();
        last_hand_yaw_ = hand_yaw_;
      }
      loop_rate.sleep();
    }
  }
 
 private:
  void TargetPoseCallback(
      const geometry_msgs::TransformStamped::ConstPtr &pose) {
    target_position_(0) = pose->transform.translation.x;
    target_position_(1) = pose->transform.translation.y;
    target_position_(2) = pose->transform.translation.z;
    ROS_INFO("Recive target position to judge close");
    // last_detect_time_ = pose->header.stamp;//这是给检测的时间，如果检测不到了的话，这个时间就用来搜寻
  }

  void DetectTimeCallback(const jsk_recognition_msgs::BoundingBox::ConstPtr &msg){
    last_detect_time_ = msg->header.stamp;
  }

  void HandPoseCallback(
      const geometry_msgs::TransformStamped::ConstPtr &pose) {
    hand_position_(0) = pose->transform.translation.x;
    hand_position_(1) = pose->transform.translation.y;
    hand_position_(2) = pose->transform.translation.z;
    hand_yaw_ = tf::getYaw(pose->transform.rotation) * 180.0 / M_PI;
    // std:: cout << "Hand yaw: " << hand_yaw_ << std::endl;
  }

  ros::NodeHandle nh_;
  ros::Subscriber target_pose_sub_;
  ros::Subscriber detect_time_sub_;
  ros::Subscriber hand_pose_sub_;
  ros::ServiceClient grip_action_;
  ros::ServiceClient kine_unlock_;
  ros::Publisher arm_search_pub_;
  Vector3d target_position_;
  Vector3d hand_position_;
  std::thread reach_moniter_thread_;
  ros::Time last_detect_time_;

  double hand_yaw_;
  double last_hand_yaw_;
  bool grip_enable_flag_;
  bool nav_ongoing_flag_;
};

int main(int argc, char **argv) {
  ros::init(argc, argv, "arm_decision_node");
  ArmDecision arm_decision;
  // ros::spin();
  return 0;
}